/* 
 *  Incub - incubate your baby robot
 *  Copyright (C) 2008 Stanislaw Szymczyk
 *  e-mail: sszymczy@gmail.com   www: http://cogville.cog-lab.com
 *
 *  This file is part of Incub.
 *
 *  Incub is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  Incub is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with Incub.  If not, see <http://www.gnu.org/licenses/>.
 * 
 */
 
#include "TactileShape.h"
#include "incubate.h"
#include <set>

using std::set;
using std::pair;
using std::vector;

class Triangle {
	public:
	Vector3D *a, *b, *c;
	Triangle(Vector3D *a, Vector3D *b, Vector3D *c) {
		this->a = a;
		this->b = b;
		this->c = c;
	}
};

struct CompareSensors {
	bool operator() (TactileSensor* s1, TactileSensor* s2) const {
		if(s1->getX() != s2->getX()) return s1->getX() < s2->getX();
		else {
			if(s1->getY() != s2->getY()) return s1->getY() < s2->getY();
			else {
				if(s1->getZ() != s2->getZ()) return s1->getZ() < s2->getZ();
				else return false;
			}
		}
	}
};

void insertSensor(std::set<TactileSensor*, CompareSensors>& sensors, TactileSensor *sensor, double x, double y, double z) {
	pair<std::set<TactileSensor*, CompareSensors>::iterator,bool> ret;
	sensor->setX(x);
	sensor->setY(y);
	sensor->setZ(z);
	ret = sensors.insert(sensor);
	if(ret.second == false) {
		delete sensor;
	}
}
	
template<>
void TactileShape<Capsule>::createSensors(TactileMatrix *matrix) {
	Capsule* thiz = dynamic_cast<Capsule*>(this);
	double _radius = thiz->getRadius();
	double _height = thiz->getLength();
	Material *mat = thiz->getMaterial();
	
	std::set<TactileSensor*, CompareSensors> sensors;
	
	double r = matrix->getTouchResolution();
	double R = _radius;
	
	double factor2 = sqrt((3.0/4.0)*r*r+R*R-sqrt(2)*R*r);
	
	double angRes = 2*M_PI;
	int angFrac = 1;
	while(angRes/angFrac > asin(r/(2*factor2))) angFrac++;
	angRes = angRes / angFrac;

	double margin = sqrt(2.0)/2 * r;
	double zRes = _height - 2*margin;
	int zFrac = 1;
	while(zRes/zFrac > matrix->getTouchResolution()) zFrac++;
	zRes = zRes / zFrac;

	for(int ang = 0; ang < angFrac; ang++) {
		double angle = ang * angRes;
		Vector3D v = Vector3D(sin(angle), cos(angle), 0).normalize() * factor2;
		for(int z = 0; z <= zFrac; z++) {
			insertSensor(sensors,new TactileSensorImpl(),v.x, v.y, -_height/2+margin+zRes*z);
		}
	}
		
	// create capsule ends by creating a sphere and splitting it into two halves

	// setup two arrays necessary for recursion subdivision
	std::vector<Triangle*> *oldM(new std::vector<Triangle*>());
	std::vector<Triangle*> *newM(new std::vector<Triangle*>());

	double factor;
	factor = sqrt(r*r + R*R - 2*R*r*sqrt(2.0/3.0));

	// store vertices
	std::vector<Vector3D*> vertices;
	
	// scale unit vectors
	Vector3D* XPLUS(new Vector3D(Vector3D(1,0,0) * factor));
	Vector3D* XMINUS(new Vector3D(Vector3D(-1,0,0) * factor));
	Vector3D* YPLUS(new Vector3D(Vector3D(0,1,0) * factor));
	Vector3D* YMINUS(new Vector3D(Vector3D(0,-1,0) * factor));
	Vector3D* ZPLUS(new Vector3D(Vector3D(0,0,1) * factor));
	Vector3D* ZMINUS(new Vector3D(Vector3D(0,0,-1) * factor));

	vertices.push_back(XPLUS);
	vertices.push_back(XMINUS);
	vertices.push_back(YPLUS);
	vertices.push_back(YMINUS);
	vertices.push_back(ZPLUS);
	vertices.push_back(ZMINUS);

	// create new triangles used for the unit octahedron
	Triangle* tri1(new Triangle(YPLUS, ZPLUS, XPLUS));
	Triangle* tri2(new Triangle(XMINUS, ZPLUS, YPLUS));
	Triangle* tri3(new Triangle(YMINUS, ZPLUS, XMINUS));
	Triangle* tri4(new Triangle(XPLUS, ZPLUS, YMINUS));
	Triangle* tri5(new Triangle(ZMINUS, YPLUS, XPLUS));
	Triangle* tri6(new Triangle(ZMINUS, XMINUS, YPLUS));
	Triangle* tri7(new Triangle(ZMINUS, YMINUS, XMINUS));
	Triangle* tri8(new Triangle(ZMINUS, XPLUS, YMINUS));
	
	// create a unit octahedron
	std::vector<Triangle*> octahedron;
	octahedron.push_back(tri1);
	octahedron.push_back(tri2);
	octahedron.push_back(tri3);
	octahedron.push_back(tri4);
	octahedron.push_back(tri5);
	octahedron.push_back(tri6);
	octahedron.push_back(tri7);
	octahedron.push_back(tri8);

	// start the initial octahedron
	*oldM = octahedron;
	
	// subdivide each starting triangle until sensor distance is lower than tactile sensor resolution
	double maxLength = (*(*oldM)[0]->a - *(*oldM)[0]->b).norm();
	while(maxLength > matrix->getTouchResolution()) {
		// reset newM
		newM->clear();
	
		/* Subdivide each polygon in the old approximation and normalize
		*  the new points thus generated to lie on the surface of the unit
		*  sphere.
		* Each input triangle with vertices labelled [0,1,2] as shown
		*  below will be turned into four new triangles:
		*
		*			Make new points
		*			    a = (0+2)/2
		*			    b = (0+1)/2
		*			    c = (1+2)/2
		*        1
		*       /\		Normalize a, b, c
		*      /  \
		*    b/____\ c		ruct new triangles
		*    /\    /\		    [0,b,a]
		*   /  \  /  \		    [b,1,c]
		*  /____\/____\	        [a,b,c]
		* 0	    a      2	    [a,c,2]
		*/
		for (unsigned i=0; i< oldM->size(); i++) {
			// get the old triangle
			Triangle* oldT = (*oldM)[i];

			// Make three new points
			Vector3D* a(new Vector3D(((*oldT->a + *oldT->c)*0.5).normalize() * factor));
			Vector3D* b(new Vector3D(((*oldT->a + *oldT->b)*0.5).normalize() * factor));
			Vector3D* c(new Vector3D(((*oldT->b + *oldT->c)*0.5).normalize() * factor));
	
			vertices.push_back(a);
			vertices.push_back(b);
			vertices.push_back(c);
	
			// construct new triangles
			Triangle* t1(new Triangle(oldT->a, b, a));
			Triangle* t2(new Triangle(b, oldT->b, c));
			Triangle* t3(new Triangle(a,b,c));
			Triangle* t4(new Triangle(a,c,oldT->c));

			// add the new triangles to the new mesh
			newM->push_back(t1);
			newM->push_back(t2);
			newM->push_back(t3);
			newM->push_back(t4);
			
			delete oldT;
		}
		maxLength = 0;
		for (unsigned i=0; i< newM->size(); i++) {
			// check length of new triangles
			Triangle* newT = (*newM)[i];
			if((*(*newM)[0]->a - *(*newM)[0]->b).norm() > maxLength) maxLength = (*(*newM)[0]->a - *(*newM)[0]->b).norm();
			if((*(*newM)[0]->a - *(*newM)[0]->c).norm() > maxLength) maxLength = (*(*newM)[0]->a - *(*newM)[0]->c).norm();
			if((*(*newM)[0]->b - *(*newM)[0]->c).norm() > maxLength) maxLength = (*(*newM)[0]->b - *(*newM)[0]->c).norm();
		}
				
		// for continued subdivision...
		*oldM = *newM;
	}

	// free unnecessary triangles
	for(vector<Triangle*>::iterator triangle = newM->begin(); triangle != newM->end(); triangle++) {
		delete *triangle;
	}

	delete oldM;
	delete newM;

	for(std::vector<Vector3D*>::iterator vertex = vertices.begin(); vertex != vertices.end(); vertex++) {
		if((*vertex)->z >=0) {
			insertSensor(sensors,new TactileSensorImpl(),(*vertex)->x, (*vertex)->y, (*vertex)->z + _height/2);
		}
		if((*vertex)->z <=0) {
			insertSensor(sensors,new TactileSensorImpl(),(*vertex)->x, (*vertex)->y, (*vertex)->z - _height/2);
		}
		delete *vertex;
	}
	
	matrix->getSensors().assign(sensors.begin(), sensors.end());
}

template<>
void TactileShape<Sphere>::createSensors(TactileMatrix *matrix) {
	Sphere* thiz = dynamic_cast<Sphere*>(this);
	double _radius = thiz->getRadius();
	Material *mat = thiz->getMaterial();

	double r = matrix->getTouchResolution();
	double R = _radius;

	std::set<TactileSensor*, CompareSensors> sensors;
	
	// setup two arrays necessary for recursion subdivision
	std::vector<Triangle*> *oldM(new std::vector<Triangle*>());
	std::vector<Triangle*> *newM(new std::vector<Triangle*>());

	double factor;
	factor = sqrt(r*r + R*R - 2*R*r*sqrt(2.0/3.0));

	// store vertices
	std::vector<Vector3D*> vertices;
	
	// scale unit vectors
	Vector3D* XPLUS(new Vector3D(Vector3D(1,0,0) * factor));
	Vector3D* XMINUS(new Vector3D(Vector3D(-1,0,0) * factor));
	Vector3D* YPLUS(new Vector3D(Vector3D(0,1,0) * factor));
	Vector3D* YMINUS(new Vector3D(Vector3D(0,-1,0) * factor));
	Vector3D* ZPLUS(new Vector3D(Vector3D(0,0,1) * factor));
	Vector3D* ZMINUS(new Vector3D(Vector3D(0,0,-1) * factor));

	vertices.push_back(XPLUS);
	vertices.push_back(XMINUS);
	vertices.push_back(YPLUS);
	vertices.push_back(YMINUS);
	vertices.push_back(ZPLUS);
	vertices.push_back(ZMINUS);

	// create new triangles used for the unit octahedron
	Triangle* tri1(new Triangle(YPLUS, ZPLUS, XPLUS));
	Triangle* tri2(new Triangle(XMINUS, ZPLUS, YPLUS));
	Triangle* tri3(new Triangle(YMINUS, ZPLUS, XMINUS));
	Triangle* tri4(new Triangle(XPLUS, ZPLUS, YMINUS));
	Triangle* tri5(new Triangle(ZMINUS, YPLUS, XPLUS));
	Triangle* tri6(new Triangle(ZMINUS, XMINUS, YPLUS));
	Triangle* tri7(new Triangle(ZMINUS, YMINUS, XMINUS));
	Triangle* tri8(new Triangle(ZMINUS, XPLUS, YMINUS));
	
	// create a unit octahedron
	std::vector<Triangle*> octahedron;
	octahedron.push_back(tri1);
	octahedron.push_back(tri2);
	octahedron.push_back(tri3);
	octahedron.push_back(tri4);
	octahedron.push_back(tri5);
	octahedron.push_back(tri6);
	octahedron.push_back(tri7);
	octahedron.push_back(tri8);

	// start the initial octahedron
	*oldM = octahedron;
	
	// subdivide each starting triangle until sensor distance is lower than tactile sensor resolution
	double maxLength = (*(*oldM)[0]->a - *(*oldM)[0]->b).norm();
	while(maxLength > matrix->getTouchResolution()) {
		// reset newM
		newM->clear();
	
		/* Subdivide each polygon in the old approximation and normalize
		*  the new points thus generated to lie on the surface of the unit
		*  sphere.
		* Each input triangle with vertices labelled [0,1,2] as shown
		*  below will be turned into four new triangles:
		*
		*			Make new points
		*			    a = (0+2)/2
		*			    b = (0+1)/2
		*			    c = (1+2)/2
		*        1
		*       /\		Normalize a, b, c
		*      /  \
		*    b/____\ c		ruct new triangles
		*    /\    /\		    [0,b,a]
		*   /  \  /  \		    [b,1,c]
		*  /____\/____\	        [a,b,c]
		* 0	    a      2	    [a,c,2]
		*/
		for (unsigned i=0; i< oldM->size(); i++) {
			// get the old triangle
			Triangle* oldT = (*oldM)[i];

			// Make three new points
			Vector3D* a(new Vector3D(((*oldT->a + *oldT->c)*0.5).normalize() * factor));
			Vector3D* b(new Vector3D(((*oldT->a + *oldT->b)*0.5).normalize() * factor));
			Vector3D* c(new Vector3D(((*oldT->b + *oldT->c)*0.5).normalize() * factor));
	
			vertices.push_back(a);
			vertices.push_back(b);
			vertices.push_back(c);
	
			// construct new triangles
			Triangle* t1(new Triangle(oldT->a, b, a));
			Triangle* t2(new Triangle(b, oldT->b, c));
			Triangle* t3(new Triangle(a,b,c));
			Triangle* t4(new Triangle(a,c,oldT->c));

			// add the new triangles to the new mesh
			newM->push_back(t1);
			newM->push_back(t2);
			newM->push_back(t3);
			newM->push_back(t4);
			
			delete oldT;
		}
		maxLength = 0;
		for (unsigned i=0; i< newM->size(); i++) {
			// check length of new triangles
			Triangle* newT = (*newM)[i];
			if((*(*newM)[0]->a - *(*newM)[0]->b).norm() > maxLength) maxLength = (*(*newM)[0]->a - *(*newM)[0]->b).norm();
			if((*(*newM)[0]->a - *(*newM)[0]->c).norm() > maxLength) maxLength = (*(*newM)[0]->a - *(*newM)[0]->c).norm();
			if((*(*newM)[0]->b - *(*newM)[0]->c).norm() > maxLength) maxLength = (*(*newM)[0]->b - *(*newM)[0]->c).norm();
		}
				
		// for continued subdivision...
		*oldM = *newM;
	}

	// free unnecessary triangles
	for(vector<Triangle*>::iterator triangle = newM->begin(); triangle != newM->end(); triangle++) {
		delete *triangle;
	}

	delete oldM;
	delete newM;

	for(std::vector<Vector3D*>::iterator vertex = vertices.begin(); vertex != vertices.end(); vertex++) {
		insertSensor(sensors,new TactileSensorImpl(),(*vertex)->x, (*vertex)->y, (*vertex)->z);
		delete *vertex;
	}
	matrix->getSensors().assign(sensors.begin(), sensors.end());
}

template<>
void TactileShape<Cylinder>::createSensors(TactileMatrix *matrix) {
	Cylinder* thiz = dynamic_cast<Cylinder*>(this);
	double _radius = thiz->getRadius();
	double _height = thiz->getLength();
	Material *mat = thiz->getMaterial();

	std::set<TactileSensor*, CompareSensors> sensors;
	
	double r = matrix->getTouchResolution();
	double R = _radius;
	
	double factor2 = sqrt((3.0/4.0)*r*r+R*R-sqrt(2)*R*r);
	
	double angRes = 2*M_PI;
	int angFrac = 1;
	while(angRes/angFrac > asin(r/(2*factor2))) angFrac++;
	angRes = angRes / angFrac;

	double margin = sqrt(2.0)/2 * r;
	double zRes = _height - 2*margin;
	int zFrac = 1;
	while(zRes/zFrac > matrix->getTouchResolution()) zFrac++;
	zRes = zRes / zFrac;

	for(int ang = 0; ang < angFrac; ang++) {
		double angle = ang * angRes;
		Vector3D v = Vector3D(sin(angle), cos(angle), 0).normalize() * factor2;
		for(int z = 0; z <= zFrac; z++) {
			insertSensor(sensors,new TactileSensorImpl(),v.x, v.y, -_height/2+margin+zRes*z);
		}
	}

	double xRes = 2*thiz->getRadius();
	int xFrac = 1;
	while(xRes/xFrac > matrix->getTouchResolution()) xFrac++;
	xRes = xRes / xFrac;

	double yRes = 2*thiz->getRadius();
	int yFrac = 1;
	while(yRes/yFrac > matrix->getTouchResolution()) yFrac++;
	yRes = yRes / yFrac;

	double margin2 = 0.5*sqrt(2.0)*matrix->getTouchResolution();

	for(int y = 0; y <= yFrac; y++) {
		for(int x = 1; x <= xFrac-1; x++) {
			double posX = -thiz->getRadius()+xRes*x;
			double posY = -thiz->getRadius()+yRes*y;
			if(sqrt(posX*posX + posY*posY) < thiz->getRadius() - margin2) {
				insertSensor(sensors,new TactileSensorImpl(), posX, posY, -thiz->getLength()/2 + margin2);
				insertSensor(sensors,new TactileSensorImpl(), posX, posY, thiz->getLength()/2 - margin2);
			}
		}
	}

	matrix->getSensors().assign(sensors.begin(), sensors.end());
}

template<>
void TactileShape<Box>::createSensors(TactileMatrix *matrix) {
	Box* thiz = dynamic_cast<Box*>(this);
	Material *mat = thiz->getMaterial();

	std::set<TactileSensor*, CompareSensors> sensors;
	
	double margin = 0.5*matrix->getTouchResolution();
	double xRes = thiz->getWidth() - 2*margin;
	int xFrac = 1;
	while(xRes/xFrac > matrix->getTouchResolution()) xFrac++;
	xRes = xRes / xFrac;

	double yRes = thiz->getHeight() - 2*margin;
	int yFrac = 1;
	while(yRes/yFrac > matrix->getTouchResolution()) yFrac++;
	yRes = yRes / yFrac;

	double zRes = thiz->getLength() - 2*margin;
	int zFrac = 1;
	while(zRes/zFrac > matrix->getTouchResolution()) zFrac++;
	zRes = zRes / zFrac;
	
	for(int y = 0; y <= yFrac; y++) {
		for(int z = 0; z <= zFrac; z++) {
			insertSensor(sensors,new TactileSensorImpl(),-thiz->getWidth()/2 + margin, -thiz->getHeight()/2+margin+yRes*y, -thiz->getLength()/2+margin+zRes*z);
			insertSensor(sensors,new TactileSensorImpl(),thiz->getWidth()/2 - margin, -thiz->getHeight()/2+margin+yRes*y, -thiz->getLength()/2+margin+zRes*z);
		}
	}

	for(int y = 0; y <= yFrac; y++) {
		for(int x = 1; x <= xFrac-1; x++) {
			insertSensor(sensors,new TactileSensorImpl(),-thiz->getWidth()/2+margin+xRes*x, -thiz->getHeight()/2+margin+yRes*y, -thiz->getLength()/2 + margin);
			insertSensor(sensors,new TactileSensorImpl(),-thiz->getWidth()/2+margin+xRes*x, -thiz->getHeight()/2+margin+yRes*y, thiz->getLength()/2 - margin);
		}
	}

	for(int z = 1; z <= zFrac-1; z++) {
		for(int x = 1; x <= xFrac-1; x++) {
			insertSensor(sensors,new TactileSensorImpl(),-thiz->getWidth()/2+margin+xRes*x, -thiz->getHeight()/2 + margin, -thiz->getLength()/2+margin+zRes*z);
			insertSensor(sensors,new TactileSensorImpl(),-thiz->getWidth()/2+margin+xRes*x, thiz->getHeight()/2 - margin, -thiz->getLength()/2+margin+zRes*z);
		}
	}
		
	matrix->getSensors().assign(sensors.begin(), sensors.end());
}
